#include "stdafx.h"
#include "Vec.h"
#include <vector>
#include "pso.h"

using std::vector;

void particlesPSO::updateNeighborPSO(int numNear){//find numNear(n) index of nearest particle wrt geodesic distance
	int i,j,k;
	vector<float>dist; vector<int>nearID;
	vec2 ref;
	for(i=0;i<(int)birds.size();i++){
		dist.clear();nearID.clear();
		ref=birds[i].P0SC;
		for(j=0;j<(int)birds.size();j++){
			if(i==j)dist.push_back(0);
			else dist.push_back(haversine(ref,birds[j].P0SC,1));
		}
		badSortId(dist,nearID,numNear+1);
		for(k=1;k<numNear;k++)
			birds[i].neighborP.push_back(nearID[k]);
	}
}

void particlesPSO::updateLBestPSO(){//update local best by comparing neighbor score
	int i,j,k, numNear;
	vector<float> scores;
	for(i=0;i<(int)birds.size();i++){
		numNear=birds[i].neighborP.size();
		scores.clear();
		for(j=0;j<numNear;j++){
			k=birds[i].neighborP[j];
			scores.push_back(birds[k].score);
		}
		k=birds[i].neighborP[findMinId(scores)];
		birds[i].lBestPos=birds[k].P0;//update position cartesian
		birds[i].lBestPosSc=birds[k].P0SC; //sc version
		birds[i].lBest=birds[k].score;//update score
	}
}

void particlesPSO::updateGBestPSO(){//update global best by comparing all particles score with previous gbest score
	int i,j,k;
	vector<float> feas; feas.clear();
	float min;
	for(i=0;i<(int)birds.size();i++)
		feas.push_back(birds[i].score);
	j=findMinId(feas);//find particle with minimum score
	if(birds[j].score<birds[0].gBest){//compare it with stored gbest
		birds[0].gBest=birds[j].score;//update score
		birds[0].gBestP0=birds[j].P0;//update position
		birds[0].gBestP0SC=birds[j].P0SC; //sc version
	}
}

void particlesPSO::updatePosPSOsc(float dScoreTh, float*stepSz, float*cons, int it, int maxIt){//update particle velocity, next position
	/*StepSz[2]= {lowerStepSz, upperStepSz};
	cons[3]={dPos weight(inertia), dLPos weight, dGPos weight};
	*/
	int i,j;
	float scalarD, randg,randl, randStep, speedStep;
	vec2 d,g,l, nextP0;
	for(i=0;i<(int)birds.size();i++){
		//evaluate score gradient to determine step size
		if(birds[i].scores.size()<2)
			birds[i].dScore=birds[i].score;
		else
			birds[i].dScore=birds[i].scores.at(birds[i].scores.size()-2)-birds[i].scores.at(birds[i].scores.size()-1);
		//update velocity -> imply direction (sc version)
		g=cons[2]*(birds[0].gBestP0SC-birds[i].P0SC);
		l=cons[1]*(birds[i].lBestPosSc-birds[i].P0SC);
		randg=rand_FloatRange(0.3,1); randl=rand_FloatRange(0.5,1);
		d=cons[0]*birds[i].dPosSc+randg*g+randl*l;
		decomposeVec2(d,scalarD, birds[i].dPosSc);
		//update speed -> imply scalar (sc version):: based on magnitude of d and current index of iteration
		speedStep=((maxIt-it+2)/maxIt+0.5)*scalarD;//early exploration and late exploitation
		//update next position (sc version)
		randStep=rand_FloatRange(0.3,1);
		if(birds[i].dScore<fabs(dScoreTh))
			birds[i].P0SC+=randStep*stepSz[1]*speedStep*birds[i].dPosSc;//higher step size
		else 
			birds[i].P0SC+=randStep*stepSz[0]*speedStep*birds[i].dPosSc;//lower step size
		//remap to pos 0-TWOPI range (robust: avoid overflow problem, if any)
		for(j=0;j<2;j++){
			if(birds[i].P0SC[j]<0)birds[i].P0SC[j]+=6.2831852;
			else if(birds[i].P0SC[j]>6.2831852)birds[i].P0SC[j]-=6.2831852;
		}
		sphereToXYZ(birds[i].P0SC,birds[i].P0);
		birds[i].P0s.push_back(birds[i].P0);
	}
}

void particlesPSO::updtNeighborPSO(int numN, float dist){
	int i, j; 
	vector <vec2> lDirs;
	vec2 dummySC; point dummyPt;
	initLocalDirs(lDirs, numN);
	for(i=0;i<birds.size();i++){//clear previous neighbor, if any
		birds[i].P0Near.clear();
		birds[i].P0NearSC.clear();
	}
	for(i=0;i<birds.size();i++)
		for(j=0;j<numN;j++){
			dummySC=birds[i].P0SC+dist*lDirs[j];
			sphereToXYZ(dummySC, dummyPt);
			birds[i].P0Near.push_back(dummyPt);
			birds[i].P0NearSC.push_back(dummySC);
		}
}

